#include "mat4f.h"



//
//	ToZero
//
void mat4f::toZero() {
	for(uint n = 0 ; n < 16 ; ++n) {
		i[n]	= 0.0f;
	}
};


//
//	ToIdentity
//
void mat4f::toIdentity() {
	toZero();
	i[0]	= 1.0f;
	i[5]	= 1.0f;
	i[10]	= 1.0f;
	i[15]	= 1.0f;
};



/* * * * * * * * * * Static Matrix creation functions * * * * * * * * * */

//
//	Rotation matrix
//
mat4f	mat4f::rotation(float degrees, const vec3f &inaxis) {
	mat4f m;

	vec3f axis = inaxis;
	axis.normalize();

	float c	= cos((float)DegToRad(degrees));
	float s = sin((float)DegToRad(degrees));

	m.i[0]  = (axis.x * axis.x) * (1.0f - c) + c;
	m.i[1]  = (axis.y * axis.x) * (1.0f - c) + (axis.z * s);
	m.i[2]  = (axis.z * axis.x) * (1.0f - c) - (axis.y * s);

	m.i[4]  = (axis.x * axis.y) * (1.0f - c) - (axis.z * s);
	m.i[5]  = (axis.y * axis.y) * (1.0f - c) + c;
	m.i[6]  = (axis.z * axis.y) * (1.0f - c) + (axis.x * s);

	m.i[8]  = (axis.x * axis.z) * (1.0f - c) + (axis.y * s);
	m.i[9]  = (axis.y * axis.z) * (1.0f - c) - (axis.x * s);
	m.i[10] = (axis.z * axis.z) * (1.0f - c) + c;

	return m;
};

//
//	Scale Matrix
//
mat4f	mat4f::scale(const vec3f &axis) {
	mat4f m;

	m.i[0]	= axis.x;
	m.i[5]	= axis.y;
	m.i[10]	= axis.z;

	return m;
};

//
//	Translation Matrix
//
mat4f	mat4f::translation(const vec3f &translation) {
	mat4f m;

	m.i[12]	= translation.x;
	m.i[13]	= translation.y;
	m.i[14]	= translation.z;

	return m;
};

//
//	Perspective projection Matrix
//
mat4f	mat4f::perspective(float znear, float zfar, float aspect, float fov) {
	mat4f m;

	const float h		= 1.0f / tan(fov * PI_OVER_360);
	float neg_depth		= znear - zfar;

	m.i[0]			= h / aspect;
	m.i[5]			= h;
	m.i[10]			= (zfar + znear) / neg_depth;
	m.i[11]			= -1.0f;
	m.i[14]			= 2.0f * (znear * zfar) / neg_depth;
	m.i[15]			= 0.0f;

	return m;
};

//
//	Orthographic projection Matrix
//	
mat4f	mat4f::orthographic(float znear, float zfar, float right, float left, float top, float bottom) {
	mat4f m;

	m.i[0]		= 2.0f / (right - left);
	m.i[5]		= 2.0f / (top - bottom);
	m.i[10]		= -2.0f / (zfar - znear);

	m.i[12]		= -(right + left) / (right - left);
	m.i[13]		= -(top + bottom) / (top - bottom);
	m.i[14]		= -(zfar + znear) / (zfar - znear);
	m.i[15]		= 1.0f;

	return m;
};

//
//	View Matrix
//
mat4f	mat4f::buildViewMatrix(const vec3f &from, const vec3f &lookingAt, const vec3f up) {
	mat4f r;
	r.setPositionVector(vec3f(-from.x, -from.y, -from.z));
	
	mat4f m;
	
	vec3f f		= (lookingAt - from);
	f.normalize();

	vec3f s		= vec3f::cross(f, up);
	vec3f u		= vec3f::cross(s, f);

	m.i[0] = s.x;
	m.i[4] = s.y;
	m.i[8] = s.z;

	m.i[1] = u.x;
	m.i[5] = u.y;
	m.i[9] = u.z;

	m.i[2]  = -f.x;
	m.i[6]  = -f.y;
	m.i[10] = -f.z;	
	
	return m * r;
};





/* * * * * * * * * * Matrix functions * * * * * * * * * */

//
//	Get position vector
//
vec3f mat4f::getPositionVector() const {
	return vec3f(i[12], i[13], i[14]);
};



//
//	Set position vector
//
void mat4f::setPositionVector(const vec3f &v) {
	i[12]	= v.x;
	i[13]	= v.y;
	i[14]	= v.z;
};



//
//	Get scaling vector
//
vec3f mat4f::getScalingVector() const {
	return vec3f(i[0], i[5], i[10]);
};



//
//	Set scaling vector
//
void mat4f::setScalingVector(const vec3f &s) {
	i[0]	= s.x;
	i[5]	= s.y;
	i[10]	= s.z;
};

/* * * * * * * * * * Matrix operators * * * * * * * * * */

//
//	Mat4f * Mat4f
//
inline mat4f mat4f::operator*(const mat4f &a) const {
	mat4f out;
	// TODO : Easy way of speeding this up?
	for(unsigned int r = 0 ; r < 4 ; ++r) {
		for(unsigned int c = 0 ; c < 4 ; ++c) {
			out.i[c + (r * 4)] = 0.0f;
			for(unsigned int i = 0 ; i < 4 ; ++i) {
				out.i[c + (r * 4)] += this->i[c + (i * 4)] * a.i[(r * 4) + i];
			}
		}
	}
	return out;
};

//
//	Vec3f * Mat4f
//
inline vec3f mat4f::operator*(const vec3f &v) const {
	vec3f vec;

	float temp;

	vec.x = v.x * i[0] + v.y * i[4] + v.z * i[8]  + i[12];
	vec.y = v.x * i[1] + v.y * i[5] + v.z * i[9]  + i[13];
	vec.z = v.x * i[2] + v.y * i[6] + v.z * i[10] + i[14];

	temp =  v.x * i[3] + v.y * i[7] + v.z * i[11] + i[15];

	vec.x = vec.x / temp;
	vec.y = vec.y / temp;
	vec.z = vec.z / temp;

	return vec;
};

//
//	Vec4f * Mat4f
//
inline vec4f mat4f::operator*(const vec4f &v) const {
	return vec4f(
		v.x * i[0] + v.y * i[4] + v.z * i[8]  +v.w  *  i[12],
		v.x * i[1] + v.y * i[5] + v.z * i[9]  +v.w  *  i[13],
		v.x * i[2] + v.y * i[6] + v.z * i[10] +v.w  *  i[14],
		v.x * i[3] + v.y * i[7] + v.z * i[11] +v.w  *  i[15]
	);
};